#include "MyRobot.h"

#define ABS(a) (a >= 0 ? a : -a)

/**
 * This code is run during the match after autonomous mode
 * is complete. This is where the majority of robot-running
 * code will occur. This file may get large when adding more
 * methods, and inherintly more complicated. Splitting this
 * page's methods into multiple pages will help keep the code
 * organized and easier to read, understand, debug, and update.
 */

void RobotDemo::OperatorControl()
{
	//Any initialization of stuff goes here, before the main
	//control loop begins.
	myRobot.SetSafetyEnabled(true);
	
	//Initialization of Driver Station
	dsLCD = DriverStationLCD::GetInstance();
	
	//Gear Shift
	bool isInFirstGear = true;
	bool hasShot = false;
	bool latchOpen = false;
	bool backDown = false, startDown = false;
	bool previousBackButtonState = false;

	//compressor.Start();
	double speeds[5] = {0,0,0,0,0};
	
	while (IsOperatorControl() && IsEnabled())
	{
		
		//test print switch status
				if(enableAuton.Get())
				{
					dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Enable Auton");
					dsLCD->UpdateLCD();
				}
				else
				{
					dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Disable Auton");
					dsLCD->UpdateLCD();
				}
				
				if(switchAuton.Get())
				{
					dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Auton 1");
					dsLCD->UpdateLCD();
				}
				else
				{
					dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Auton 2");
					dsLCD->UpdateLCD();
				}
		
		//this is causing one of the talon's to 'spaz', check into it, try swapping PWM cables to rule out
		//a bad talon (for whatever reason) or bad PWM cable, etc.
		/*myRobot.TankDrive((fabs(leftStick.GetY()) > JOYSTICKDEADSPACE ? leftStick.GetY() : 0.0),
						  (fabs(rightStick.GetY()) > JOYSTICKDEADSPACE ? rightStick.GetY() : 0.0));
	*/
		//if that doesn't work, uncomment this (and comment out the above) to rule out the code above as the issue.
		/*
		if ((leftStick.GetY() > JOYSTICKDEADSPACE || leftStick.GetY() < -JOYSTCKDEADSPACE) &&
			(rightStick.GetY() > JOYSTICKDEADSPACE || rightStick.GetY() < -JOYSTICKDEADSPACE))
			myRobot.TankDrive(leftStick.GetY(), rightStick.GetY());
		else if (leftStick.GetY() < JOYSTICKDEADSPACE && leftStick.GetY() > -JOYSTICKDEADSPACE &&
				(rightStick.GetY() > JOYSTICKDEADSPACE || rightStick.GetY() < -JOYSTICKDEADSPACE))
			myRobot.TankDrive(0.0, rightStick.GetY());
		else if ((leftStick.GetY() > JOYSTICKDEADSPACE || leftStick.GetY() < -JOYSTCKDEADSPACE) &&
				rightStick.GetY() < JOYSTICKDEADSPACE&& rightStick.GetY() > -JOYSTICKDEADSPACE)
			myRobot.TankDrive(leftStick.GetY(), 0.0);
		else myRobot.TankDrive(0.0, 0.0);
		
		
		//if it's still not working, try custom ABS function defined above to replicate the first one above
		
		myRobot.TankDrive((ABS(leftStick.GetY()) > JOYSTICKDEADSPACE ? leftStick.GetY() : 0.0),
						  (ABS(rightStick.GetY()) > JOYSTICKDEADSPACE ? rightStick.GetY() : 0.0));
		
		*/ 
		
		
		
		//LIGHT CODE AND COMPRESSOR DISPLAY
	/*	if (compressor.GetPressureSwitchValue())
		{
			ledSpike.Set(Relay::kOn);
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Compressor Off");
			dsLCD->UpdateLCD();
		}
		else 
		{
			ledSpike.Set(Relay::kOff);
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Compressor On");
			dsLCD->UpdateLCD();
		}
		*/
		
		
		/*
		//LATCH
		if (xbox.GetRawButton(XBOX_BACK))
		{
			if (previousBackButtonState == false)
			{
				if (latchOpen == true) // && !armDown.Get()
				{
					latch.Set(false);
					retractLatch.Set(true);
					latchOpen = !latchOpen;
					//backDown = true;
					//Print to DS
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Back Pressed");
					dsLCD->UpdateLCD();
				}
				else if(latchOpen == false)
				{
					latch.Set(true);
					retractLatch.Set(false);
					latchOpen = !latchOpen;
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Back Released");
					dsLCD->UpdateLCD();
					//backDown = false;
				}
			}
			previousBackButtonState = true;
		}
		else
		{
			previousBackButtonState = false;
		}	
		*/
		
		/*
		//Emergency latch move
		if (xbox.GetRawButton(XBOX_B))
		{
			if (!backDown)
				{
					latch.Set(!latchOpen);
					retractLatch.Set(latchOpen);
					latchOpen = !latchOpen;
					backDown = true;
					//Print to DS
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "B Pressed");
					dsLCD->UpdateLCD();
				}
		}else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "B Released");
			dsLCD->UpdateLCD();
			backDown = false;
		}
		*/
		
		/*
		//SHOOTERS
		if (xbox.GetRawButton(XBOX_START))
		{
			if (!startDown)
			{
				launchers.Set(!hasShot);
				retractLaunchers.Set(hasShot);
				hasShot = !hasShot;
				startDown = true;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Start Pressed");
				dsLCD->UpdateLCD();
			}
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Start Released");
			dsLCD->UpdateLCD();
			startDown = false;
		}
		
		//MORE SHIFTING
		if (rightStick.GetRawButton(2))
		{
			//FIRST GEAR
			if (!isInFirstGear)
			{
				shifters.Set(true);
				retractShifters.Set(false);
				isInFirstGear = true;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "First Gear");
				dsLCD->UpdateLCD();
			}
		}
		
		//MORE SHIFTING
		if (rightStick.GetRawButton(3))
		{
			//SECOND GEAR
			if (isInFirstGear)
			{
				shifters.Set(false);
				retractShifters.Set(true);
				isInFirstGear = false;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Second Gear");
				dsLCD->UpdateLCD();
			}
		}
		*/
		
		//motor2 ARM
		if (xbox.GetRawButton(XBOX_Y))
		{
			if(speeds[0] < 1.0)
				speeds[0] += .1;
			else
				speeds[0] = 0;
			
			motor1.SetSpeed(0.0);
			motor2.SetSpeed(0.0);
			motor3.SetSpeed(0.0);
			motor4.SetSpeed(0.0);
			motor5.SetSpeed(0.0);
			
			motor1.SetSpeed(speeds[0]);

		} 
		else if (xbox.GetRawButton(XBOX_A))
		{
			if(speeds[1] < 1.0)
							speeds[1] += .1;
						else
							speeds[1] = 0;
						
						motor1.SetSpeed(0.0);
						motor2.SetSpeed(0.0);
						motor3.SetSpeed(0.0);
						motor4.SetSpeed(0.0);
						motor5.SetSpeed(0.0);
						
						motor2.SetSpeed(speeds[1]);									
		} 
		else if (xbox.GetRawButton(XBOX_X))
		{
			if(speeds[2] < 1.0)
							speeds[2] += .1;
						else
							speeds[2] = 0;
						
						motor1.SetSpeed(0.0);
						motor2.SetSpeed(0.0);
						motor3.SetSpeed(0.0);
						motor4.SetSpeed(0.0);
						motor5.SetSpeed(0.0);
						
						motor3.SetSpeed(speeds[2]);
			
		} 
		else if (xbox.GetRawButton(XBOX_B))
		{
			if(speeds[3] < 1.0)
							speeds[3] += .1;
						else
							speeds[3] = 0;
						
						motor1.SetSpeed(0.0);
						motor2.SetSpeed(0.0);
						motor3.SetSpeed(0.0);
						motor4.SetSpeed(0.0);
						motor5.SetSpeed(0.0);
						
						motor4.SetSpeed(speeds[3]);	
									
		} 
		else if (xbox.GetRawButton(XBOX_START))
		{
			if(speeds[4] < 1.0)
							speeds[4] += .1;
						else
							speeds[4] = 0;
						
						motor1.SetSpeed(0.0);
						motor2.SetSpeed(0.0);
						motor3.SetSpeed(0.0);
						motor4.SetSpeed(0.0);
						motor5.SetSpeed(0.0);
						
						motor5.SetSpeed(speeds[4]);
		}
		else
		{
			
		}
		
		
		
		if (liftUp.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Upper Limit");
			dsLCD->UpdateLCD();
		}
		if (!liftDown.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Lower Limit");
			dsLCD->UpdateLCD();
		}
		
		
		/*
		// Capture Ball wheels
		if (xbox.GetRawButton(XBOX_LBUMP) || xbox.GetRawButton(XBOX_RBUMP))
		{
			captureBall.Set(-0.5);
		}
		else
		{
			captureBall.Set(0.0);
		}
		*/
		
		dsLCD->Clear();
		UpdateSmartDashboard();	//update the smart dashboard after everything is updated
		Wait(0.005); // Wait for a motor update time
	}
	


}

